/*
 * CricketM.nc
 * Michel Goraczko <michelg@csail.mit.edu>
 *
 * Cricket modules 
 *
 *   
 */

/*
      This software may be used and distributed according to the terms
      of the GNU General Public License (GPL), incorporated herein by reference.
      Drivers based on this skeleton fall under the GPL and must retain
      the authorship (implicit copyright) notice.

      This program is distributed in the hope that it will be useful, but
      WITHOUT ANY WARRANTY; without even the implied warranty of
      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
      General Public License for more details.
*/


module CricketM {
  provides {
    interface StdControl;
  }
  uses {
    // Timer
    interface Timer as SingleTimer;
    interface Timer as BeaconTimer;
    interface Random;
    interface Leds;
    interface WriteData;
    interface ReadData;
    interface StdControl as RadioControl;
    interface BareSendMsg as RadioSend;
    interface ReceiveMsg as RadioReceive;
    interface RadioCoordinator as RadioSendCoord;
    interface RadioCoordinator as RadioReceiveCoord;
    interface BeacManage as BeaconsManage;
    interface StdControl as BeaconsControl;
    interface StdControl as OnboardTempcontrol;
    interface HardwareId;
    interface UltrasoundControl;
    interface ADC as OnboardTemp;
    interface Time;
    interface Serial;
    interface StdControl as CarControl;
    interface Car;

    async command uint8_t GetRxBitOffset();
  }
}
implementation {

  // Global configuration 
  norace CricketConfig_s CricketConfig = {"2.0\0",
 				  "\0\0\0\0\0\0\0\0",
				  MODE_LISTENER,
				  SPEED_OF_SOUND,
				  TIMER_OFFSET,
				  COMPENSATION_VAL,
				  DISTANCE_MULT,
				  MAX_ALLOWED_TIMER_VAL,
				  AVG_BEACON_INTERVAL_TIME,
				  DELTA_BEACON_INTERVAL_TIME,
				  MIN_BEACON_INTERVAL_TIME, 
				  MAX_BEACON_INTERVAL_TIME,
				  MAX_US_TRAVEL_TIME, 
				  MAX_NUMBER_OF_BEACONS,
				  TIME_MS,
				  DIST_METRIC,
				  TEMP_C,
				  3,
				  0,
				  0,
				  0,
				  0,
				  FALSE,
				  TRUE,
				  TRUE,
				  TRUE,
				  TRUE,
				  TRUE,
				  TRUE,
				  FALSE,
				  TRUE,
				  FALSE,
				  FALSE,
				  127};
	
  
  // Serial id on the local beacon
  uint8_t serialId[8];
  // Last space/id receive 
  norace uint8_t last_id[8];
  norace uint8_t last_space[8];
  norace uint8_t beac_id[8];
  norace uint8_t beac_space[8];
  norace uint16_t last_beacon_coord_x = 0;
  norace uint16_t last_beacon_coord_y = 0;
  norace uint16_t last_beacon_coord_z = 0;
  norace int8_t last_beacon_temp = 0;
  norace bool last_beacon_tb = FALSE;
  //custom ID
  int customID = 0;
  int DistanceSend = 0;

  // State variable
  uint8_t listing_count = 0;
  uint8_t got_id = 0;
  uint8_t detector_is_on = 0;
  uint8_t reported = 1;
  uint16_t last_time = 0;

  // RF bit receive compensation 
  uint8_t compensation = 0;
  uint16_t us_time = 0;

  // Space ID
  uint8_t tmpConfig[sizeof(CricketConfig_s)];
  uint8_t GroupId = 78;
  // Beacon message
  TOS_Msg   beaconmsg;
  struct CricketBeacon *beacondata = (struct CricketBeacon *)beaconmsg.data;
  uint8_t CricketId = 0; // not set
  uint8_t DestAll = 90;
  // Desyncronisation variable

  uint16_t DesyncDelay;
  norace bool ReadyToSend;
  
  // init fase, define wait time before sending a message
  uint16_t DesyncDelayInit;
  		
  

  uint8_t carDrive;

  bool readTestSwitch() {
		if ((_BV(7) & PINE) == 0) {
			return FALSE;
		} else {
			return TRUE;
		}
  }
  
  /**
   * Initialize the component.
   * 
   * @return Always returns <code>SUCCESS</code>
   **/
 
  command result_t StdControl.init() {

    int i;
    // Init Leds
    call Leds.init();

    // Read saves configuration from flash
    call ReadData.read(0, &tmpConfig[0], sizeof(struct CricketConfig_s));
    //TOSH_uwait(1000);
    // LEDUIT call Leds.yellowOn();
    // LEDUIT call Leds.greenOn();
    // LEDUIT call Leds.redOn();
// eenmalige mode initialisatie
    if (readTestSwitch())
        CricketConfig.run_mode = MODE_BEACON;
    else
        CricketConfig.run_mode = MODE_LISTENER;
        
    // Enable RS-232 Chip  
    TOSH_SET_BAT_MON_PIN();

    if (((CricketConfig.run_mode == MODE_BEACON) && !readTestSwitch()) || CricketConfig.run_mode == MODE_LISTENER) {
   	    TOSH_CLR_US_IN_EN_PIN();
    } else {
  	    TOSH_SET_US_IN_EN_PIN();
    }
    
    // Wait until the RS-232 start
    TOSH_uwait(2000);

    // Prepare the beacon message
    for(i=0;i<8;i++) 
        beaconmsg.data[i] = CricketConfig.spaceid[i];

    beaconmsg.length = 23;
    beaconmsg.type = SYN1;
    beaconmsg.addr = TOS_BCAST_ADDR; 	
    beacondata->x = 0;
    beacondata->y = 0;
    beacondata->z = 0;
    beacondata->temp = 0;
    beacondata->tb = FALSE;

    // Init the rest
    atomic {
            ReadyToSend = 0;
    }
    
    // Read the serialid of the listener
    call HardwareId.read(serialId);
    // Init radio 	
    call RadioControl.init();
    // Init serial port
    call Serial.SetStdoutSerial();
    // Init beacon tracker
    call BeaconsControl.init();

    call Random.init();
	DesyncDelayInit = call Random.rand();
	DesyncDelayInit = DesyncDelay % 6000;// this is half the time toshuwait will be called twice with Desyncdelayinit as argument
	
    // Init temp sensor
    call OnboardTempcontrol.init();

    // Initialize the car
    call CarControl.init();
    
    return SUCCESS;
  }


  /**
   * Start things up.  This just sets the rate for the clock component.
   * 
   * @return Always returns <code>SUCCESS</code>
   **/
  command result_t StdControl.start() {

    if (CricketConfig.use_temp_sensor) {
	    call OnboardTempcontrol.start();
	    call OnboardTemp.getData();
    }
    if (CricketConfig.run_mode == MODE_BEACON) 
    {
		//call BeaconTimer.start(TIMER_ONE_SHOT, 2000);
    }
    if (CricketConfig.run_mode == MODE_LISTENER) 
    {
    	    call RadioControl.start();
	    call BeaconsControl.start();
    }
    call Leds.yellowOff();
    call Leds.greenOff();
    call Leds.redOff();

    // Start the car
    call CarControl.start();        
    call SingleTimer.start(TIMER_ONE_SHOT, MOVING_TIME);
    carDrive = 0;

    return SUCCESS;
  }

  /**
   * Halt execution of the application.
   * This just disables the clock component.
   * 
   * @return Always returns <code>SUCCESS</code>
   **/
  command result_t StdControl.stop() {
    call CarControl.stop();
    call SingleTimer.stop();
    call BeaconsControl.stop();
    call BeaconTimer.stop();
    call RadioControl.stop();
    return SUCCESS;
  }


   /**   
    * SingleTimer.
    *
    * @return Always returns <code>SUCCESS</code>
    **/
    event result_t SingleTimer.fired()
    {        // Stop the engine of the car
        if (carDrive == 1)
        {
         call Car.stop();
         carDrive = 0;
			// klaar met rijden, zend bericht zodat iedereen afstand kan bepalen
			beaconmsg.data[0] = 22; // iedereen moet afstand verzenden met id vertraging
			beaconmsg.data[1] =  0; // 0 staat voor de auto t.o.v. waarvan de andere auto's hebben gemeten
			beaconmsg.data[2] = 154; // alle auto's moeten afstanden verzenden
			beaconmsg.data[3] = CricketId;
			beaconmsg.data[4] = GroupId;
			//beaconmsg.addr = serialId; 	
			UARTOutput(DBG_ERROR,"Sending opcode send distance to all the cars!");	
			call RadioSend.send(&beaconmsg);
        }

        
        // LEDUIT call Leds.redToggle();
        //call SingleTimer.start(TIMER_ONE_SHOT, MOVING_TIME);
        return SUCCESS;
    }
  
  /**
   * Send the beacon message
   *
   * @return Always returns <code>SUCCESS</code>
   **/

  event result_t BeaconTimer.fired()
  {
    int result = 0;
    int count = 0;
    uint8_t i,z;
    //uint16_t FailedAttemptComp;
    //char OutputMsg[128];

    call RadioControl.start();
    TOSH_uwait(2000);

    //if (CricketConfig.run_mode == MODE_BEACON) {
      while ( count < CricketConfig.max_beacon_number - 1)
      {
          atomic {
                  ReadyToSend = 0;
          }
          // Add desync to help with hidden terminal
          DesyncDelay = call Random.rand();
          DesyncDelay = DesyncDelay % DESYNC_DELAY;

          // Listen for MAX_US_TRAVEL_TIME ms + extra time
	  z = (CricketConfig.max_us_travel_time + DesyncDelay) / 5000;
	  for (i = 0; i < z; i++) 
	  {
          	TOSH_uwait(5000);
	  }
          TOSH_uwait(CricketConfig.max_us_travel_time + DesyncDelay - (5000 * z));

          //TOSH_uwait(CricketConfig.max_us_travel_time + DesyncDelay);

          if (ReadyToSend == 0) {
            // Send the location
	    if (CricketConfig.use_temp_sensor) 
		beacondata->temp = CricketConfig.local_temp;
	    beacondata->tb = readTestSwitch();
            result = call RadioSend.send(&beaconmsg);
            if (result == SUCCESS)
            {
                // LEDUIT call Leds.redOff();
                // LEDUIT call Leds.greenToggle();
                break;
            }
          }
          // LEDUIT call Leds.redOn();
          count++;
      }

      // reschedule for next beacon
      DesyncDelay = call Random.rand();
      DesyncDelay = DesyncDelay %  CricketConfig.delta_beacon_interval_time;
      //FailedAttemptComp = count * CricketConfig.max_us_travel_time;
      //if (FailedAttemptComp > CricketConfig.min_beacon_interval_time) FailedAttemptComp = 1;
      //call BeaconTimer.start(TIMER_ONE_SHOT, CricketConfig.min_beacon_interval_time - FailedAttemptComp + DesyncDelay);
      //call BeaconTimer.start(TIMER_ONE_SHOT, CricketConfig.min_beacon_interval_time + DesyncDelay);
      //sprintf(OutputMsg,"D2 desync=%i count=%i delta=%i\r\n",DesyncDelay, count,CricketConfig.delta_beacon_interval_time);
      //UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    //}

    return SUCCESS;
  }


  event result_t RadioSend.sendDone(TOS_MsgPtr rmsg, result_t success) {
        call RadioControl.stop();
	return success;
  }

  async event void RadioSendCoord.startSymbol(uint8_t bitsPerBlock, uint8_t offset, TOS_MsgPtr msgBuff) {}
  async event void RadioSendCoord.blockTimer() {}
  async event void RadioSendCoord.byte(TOS_MsgPtr msg, uint8_t cnt)
  {
        if (cnt == (offsetof(struct TOS_Msg,data) + 3)
                && (msg->type == SYN1)){
		if (CricketConfig.run_mode == MODE_BEACON)
		{
                	call UltrasoundControl.SendPulse();
		}
        }
  }

  async event void RadioReceiveCoord.startSymbol(uint8_t bitsPerBlock, uint8_t offset, TOS_MsgPtr msgBuff) {};
  async event void RadioReceiveCoord.blockTimer() {};
  async event void RadioReceiveCoord.byte(TOS_MsgPtr msg, uint8_t cnt)
  {
      if (cnt == (offsetof(struct TOS_Msg,type) +
                  sizeof(((struct TOS_Msg*)0)->type)) &&
              msg->type == SYN1) {
	  if (CricketConfig.run_mode == MODE_LISTENER) {
            call UltrasoundControl.StartDetector(CricketConfig.max_allowed_timer_val);
            atomic {
  	  	  got_id = 0;
  		  us_time = 0;
	          atomic compensation = call GetRxBitOffset();
		  detector_is_on = 1;
	    }
            // LEDUIT call Leds.greenOn();
	  } else {
	    atomic {
		    ReadyToSend = 1;
	    }
            // LEDUIT call Leds.yellowToggle();
	  }
		
      }
  }

  /**
   * Read the temperature from the onboard sensor
   *
   * @return Always returns <code>SUCCESS</code>
   **/

  async event result_t OnboardTemp.dataReady(uint16_t data){
    float temp;
    float x;
    x = 10000 / (1023 - data);
    x = x * data;
    temp = (float) -26.308 * log(x) + 267.25;
    CricketConfig.local_temp =  temp;
    return SUCCESS;
  }

  /**
   * Task that report the distances received temperature
   *
   * @return Always returns <code>SUCCESS</code>
   **/
  task void ReportPulse()
  {
      float 	speed_of_sound = 0;
      uint8_t 	msglenght = 0;
      uint8_t	comp;
      uint16_t 	ajusted_timer = 0;
      uint16_t 	timer;
      uint32_t 	distance = 0;
      uint32_t 	timestamp;
      //ASCII_Msg OutputMsg;
      char OutputMsg[255];

      atomic {
          comp = compensation;
          timer = last_time;
      }
      // Call to read the local temperature 
      call OnboardTemp.getData();
      ajusted_timer = timer + COMPENSATION_VAL * comp;

      if ((ajusted_timer) > CricketConfig.timer_offset && timer != 0) {

	  if (CricketConfig.use_temp_sensor) { 
		  // feet / second
		  //speed_of_sound = ((1087 * sqrt(273 + CricketConfig.local_temp )) / 16.52);
		  // meters / second
		  speed_of_sound = (331.4 + 0.60 * CricketConfig.local_temp) / 1000;	
	  
		  distance = (uint32_t) (((ajusted_timer - CricketConfig.timer_offset) / 10) * speed_of_sound );
	  } else {
          	  distance = (uint64_t) (ajusted_timer - CricketConfig.timer_offset) / DISTANCE_MULT;
	  }
	  call BeaconsManage.NewMeasurement(last_id, distance);
	  
      } else {
	  distance = 0;
    	  // LEDUIT call Leds.yellowToggle();
      }
      
           if (CricketConfig.outputformat == 1) {
          	msglenght = sprintf(OutputMsg,
                	  "$Cricket2,ver=3.0,id=45,dist=%d,duration=%d,time=%d\r\n",
	                  (uint16_t) distance,
        	          (uint16_t) ajusted_timer,
                	  (uint16_t) timer);
           } else if (CricketConfig.outputformat == 2) {
		// Output format no 3 is used by Allen's demo and pycricket
          	msglenght = sprintf(OutputMsg,
                	  "$Cricket2,ver=3.0,space=%s,id=45",
			  (char *) &last_space);
		if (distance != 0) {
          		msglenght += sprintf(OutputMsg + msglenght, ",dist=%x,duration=%d",
				(uint16_t) distance,
        	          	(uint16_t) ajusted_timer);
		}
		msglenght += sprintf(OutputMsg + msglenght, "\r\n");
           } else if (CricketConfig.outputformat == 3) {
	  	// TODO add unit conversion NOTE: %E
		// Output format no 4 is the offical for this release this is the only format that support the custom output
		if (CricketConfig.output_vr) msglenght += sprintf(OutputMsg + msglenght, "VR=%s,", CricketConfig.version);
		if (CricketConfig.output_id) 
		{
			msglenght += sprintf(OutputMsg + msglenght, "ID=%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x,", 
			  			last_id[0],last_id[1],
						last_id[3],last_id[3],
					    	last_id[4],last_id[5],
						last_id[6],last_id[7]);
		}
		if (CricketConfig.output_sp) msglenght += sprintf(OutputMsg + msglenght, "SP=%s,", (char *) &last_space);
		if (distance != 0) {
			if (CricketConfig.distance_units == DIST_IMPERIAL) {
				if (CricketConfig.output_db) msglenght += sprintf(OutputMsg + msglenght, "DB=%d,", (uint16_t) (distance * 0.3937));
			} else {
				if (CricketConfig.output_db) msglenght += sprintf(OutputMsg + msglenght, "DB=%d,", (uint16_t) distance);
			}
			if (CricketConfig.output_dr) msglenght += sprintf(OutputMsg + msglenght, "DR=%d,", (uint16_t) (ajusted_timer - CricketConfig.timer_offset));
			if (CricketConfig.output_tm) msglenght += sprintf(OutputMsg + msglenght, "TM=%d,", (uint16_t) timer);
		}
		if (CricketConfig.output_ts) {
			timestamp = call Time.getLow32();
			msglenght += sprintf(OutputMsg + msglenght, "TS=%lu,", timestamp);
		}
		if (CricketConfig.output_pc) {
			msglenght += sprintf(OutputMsg + msglenght, "PC=(%d,%d,%d),", 
					     last_beacon_coord_x,
					     last_beacon_coord_y,
					     last_beacon_coord_z);
		}
		if (CricketConfig.output_tb) {
			
			if (last_beacon_tb) {
				msglenght += sprintf(OutputMsg + msglenght, "TB=1,");
			} else {
				msglenght += sprintf(OutputMsg + msglenght, "TB=2,");
			}
		}
		if (CricketConfig.temp_units == TEMP_C) {
			if (CricketConfig.output_tp) msglenght += sprintf(OutputMsg + msglenght, "TP=%d", (uint16_t) last_beacon_temp);
		} else { 
			if (CricketConfig.output_tp) msglenght += sprintf(OutputMsg + msglenght, "TP=%d", (uint16_t) (1.8 * last_beacon_temp + 32) );
		}
		if (!CricketConfig.output_tp) msglenght -= 1;
		msglenght += sprintf(OutputMsg + msglenght, "\r\n");
	  }
		if (DistanceSend == 1)
			{			
			call Leds.greenOn();
			call Leds.yellowOff();
			call Leds.redOn();
			
			if(distance>255) distance = 255; // kan een probleem zijn ivm 128 ipv 264
			if(distance<1) distance = 1; // kan waarschijnlijk niet  0 mag niet!
			distance = distance - 127;
			beaconmsg.data[0] = 66;// opcode RF met afstand
			beaconmsg.data[1] = distance;
			beaconmsg.data[2] = 15;// destination = satelliet = 15
			beaconmsg.data[3] = CricketId;
			beaconmsg.data[4] = GroupId; //groepsid
			//beaconmsg.addr = serialId; 	
			TOSH_uwait(100*CricketId);// zodat niet meerdere  berichten tegelijk worden verzonden
			UARTOutput(DBG_ERROR,"Sending last distance");	
			
			// LEDUIT call Leds.redToggle();
			// LEDUIT call Leds.greenToggle();
			call BeaconTimer.start(TIMER_ONE_SHOT, 2000);
			
			DistanceSend = 0;
			}
	  if (CricketConfig.outputformat != 0) 
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);;
		

      atomic {
            reported = 1;
      }
  }

  async event result_t UltrasoundControl.PulseDetected(uint16_t timer)
  {
      atomic {
            if (us_time == 0) {
                // LEDUIT call Leds.greenOff();
                us_time = timer;
            }
      }
      return SUCCESS;
  }

  void try_post_report() {
        uint8_t i;
        if (got_id && !detector_is_on && reported) { 
            last_time = us_time;
            for (i=0; i<8; i++)
                last_id[i] = beac_id[i];
            for (i=0; i<8; i++)
                last_space[i] = beac_space[i];
            post ReportPulse();
            reported = 0;
        }
  }

  async event result_t UltrasoundControl.DetectorTimeout()
  {// h
      if (CricketConfig.run_mode == MODE_LISTENER) {
        atomic {
              detector_is_on = 0;
              if (us_time == 0) {
                  // LEDUIT call Leds.greenOff();
              }
              try_post_report();
        }
      }
      return SUCCESS;
  }

  event TOS_MsgPtr RadioReceive.receive(TOS_MsgPtr data) {
 

    uint8_t i;
	char OutputMsg[256];
    //ASCII_Msg OutputMsg;
	UARTOutput(DBG_ERROR,"test");
	call Leds.greenToggle();
    if (!data->crc)
        return data;// ?????????????????????????????????????????????????????/
		call Leds.greenToggle();
        /* TODO: check CRC -- if it fails, no beacons will be trusted
           for a while and we will not be clear to send our own */
		   /*
		if(data->data[4] != GroupId)
			{return data;} // niet voor deze groep bedoeld
		if (data->data[2] != CricketId && CricketId != 0 && data->data[2] != DestAll)
			{ 
			return data;} // niet voor deze cricket bedoeld
		
		call Leds.greenOn();
		call Leds.yellowOn();
		call Leds.redOn();
		
		if(data->data[0] == 11)
		{// 11-> rijden
			call Leds.greenOn();
			call Leds.yellowOn();
			call Leds.redOff();
			call Car.drive();
			carDrive = 1;
			call SingleTimer.start(TIMER_ONE_SHOT, MOVING_TIME);
		}
		else
		if(data->data[0] == 22)
		{// 22-> laatste afstand verzenden
			DistanceSend = 1;
			call Leds.greenOn();
			call Leds.yellowOff();
			call Leds.redOff();
			

		}
		else
		if(data->data[0] == 33)
		{// 22-> start initialisatie procedure
			call Leds.greenOff();
			call Leds.yellowOn();
			call Leds.redOff();
			
			//for (i=0; i<8; i++) beac_id[i] = beac->Id[i];
			TOSH_uwait(DesyncDelayInit);// 1e delay, 2e nodig i.v.m.. maximale delay toshuwait per keer
			TOSH_uwait(DesyncDelayInit);// 2e delay, 2e nodig i.v.m.. maximale delay toshuwait per keer
			CricketConfig.run_mode = MODE_BEACON;
			beaconmsg.data[0] = 66;
			beaconmsg.data[1] =  255; // 255 is onbruikbaar, 0 zou betekenen, dit is de gereden car
			beaconmsg.data[2] = 20;
			beaconmsg.data[3] = 0;// nog niet bekend
			beaconmsg.data[4] = GroupId;
			// beaconmsg.addr = serialId; 	NOG AANPASSEN, DIT PAST NIET!!! HET IS EEN LANGERE ARRAY
			UARTOutput(DBG_ERROR,"Init procedure, send radio message for the list to the satteliet");	
			call BeaconTimer.start(TIMER_ONE_SHOT, 2000);
			//call RadioSend.send(&beaconmsg);
			
			// LEDUIT call Leds.greenToggle();
			CricketConfig.run_mode = MODE_LISTENER;
		}
		else
		if(data->data[0] == 44)
		{// 22-> id toekennen
			CricketId = data->data[1];
			UARTOutput(DBG_ERROR,"Init procedure, cricket id set");	
			call Leds.greenOff();
			call Leds.yellowOff();
			call Leds.redOn();
		}
		/*else
		if(data->data[0] == 55)
		{// 22-> laatste afstand verzenden, een voor een op ID
			if(data->data[3] == (CricketId -1 ))
			{
				if(distance>200) distance = 200; // kan een probleem zijn ivm 128 ipv 264
				if(distance<1) distance = 1; // kan waarschijnlijk niet
				beaconmsg.data[0] = 55;
				beaconmsg.data[1] =  (uint8_t)distance;
				beaconmsg.data[2] = 1;
				beaconmsg.data[3] = CricketId;
				beaconmsg.data[4] = GroupId;
				//beaconmsg.addr = serialId; 	
				UARTOutput(DBG_ERROR,"Sending last distance by id volgorde");	
				call RadioSend.send(&beaconmsg);
				// LEDUIT call Leds.redToggle();
				// LEDUIT call Leds.greenToggle();
			}
		}*/
		
    if (CricketConfig.run_mode == MODE_LISTENER) {
		

			
		
	
		sprintf(OutputMsg,"blablablabla %i",data->data[0]);
		UARTOutput(DBG_ERROR,"%s",OutputMsg);
		//car drive if data[0] = 23


	    if (data->type == SYN1) {
	        struct CricketBeacon * beac = (struct CricketBeacon *)data->data;
      		// LEDUIT call Leds.yellowOff();
		for (i=0; i<8; i++)
	                beac_id[i] = beac->Id[i];
		for (i=0; i<8; i++)
	                beac_space[i] = beac->Space[i];
		last_beacon_coord_x = beac->x;
		last_beacon_coord_y = beac->y;
		last_beacon_coord_z = beac->z;
		last_beacon_temp = beac->temp;
		last_beacon_tb = beac->tb;
	        atomic {
	            got_id = 1;
	            try_post_report();
				}
	        }
	    
    }
    return data;
  }

  event result_t HardwareId.readDone(uint8_t *id, result_t success) {

	if (success == SUCCESS) {
   		int i;
		for (i = 0;i < HARDWARE_ID_LEN;i++) {
        		serialId[i] = id[i];
			beaconmsg.data[i+8] = id[i];
		}
	}
	return SUCCESS;
  }

  event result_t WriteData.writeDone(uint8_t *pWriteBuffer,uint32_t writeNumBytesWrite,result_t result)
  {
	// LEDUIT call Leds.redOff();
	return SUCCESS;
  }
  event result_t ReadData.readDone(uint8_t *pReadBuffer,uint32_t readByteCount,result_t result)
  {
  	struct CricketConfig_s *tmp = (CricketConfig_s *) pReadBuffer;
	// LEDUIT call Leds.redOn();
        	
	if (readByteCount == sizeof(struct CricketConfig_s)) 
	{ 	
		if (tmp->version[0] == '2' && tmp->version[1] == '.' && tmp->version[2] == '0') {
			int i;
			CricketConfig.run_mode = tmp->run_mode; 
			CricketConfig.timer_offset = tmp->timer_offset;	
			//CricketConfig.compensation_val = tmp->compensation_val;
			//CricketConfig.distance_mult = tmp->distance_mult;
			CricketConfig.max_allowed_timer_val = tmp->max_allowed_timer_val;
			CricketConfig.delta_beacon_interval_time = tmp->delta_beacon_interval_time;
			CricketConfig.min_beacon_interval_time = tmp->min_beacon_interval_time;
			CricketConfig.max_beacon_interval_time = tmp->max_beacon_interval_time;
			CricketConfig.avg_beacon_interval_time = (CricketConfig.max_beacon_interval_time + CricketConfig.min_beacon_interval_time ) / 2;
			CricketConfig.max_us_travel_time = tmp->max_us_travel_time;
			CricketConfig.time_units = tmp->time_units;
			CricketConfig.distance_units = tmp->distance_units;
			CricketConfig.outputformat = tmp->outputformat;
			CricketConfig.x = tmp->x;
			CricketConfig.y = tmp->y;
			CricketConfig.z = tmp->z;
			CricketConfig.use_temp_sensor = tmp->use_temp_sensor;
			CricketConfig.output_vr = tmp->output_vr;
			CricketConfig.output_db = tmp->output_db;
			CricketConfig.output_dr = tmp->output_dr;
			CricketConfig.output_sp = tmp->output_sp;
			CricketConfig.output_id = tmp->output_id;
			CricketConfig.output_tm = tmp->output_tm;
			CricketConfig.output_tp = tmp->output_tp;
			CricketConfig.output_ts = tmp->output_ts;
			CricketConfig.output_pc = tmp->output_pc;
			CricketConfig.ultrasound_gain = tmp->ultrasound_gain;
			call UltrasoundControl.SetGain(CricketConfig.ultrasound_gain);
			// Update internal state with the new variable
    			if (((CricketConfig.run_mode == MODE_BEACON) && !readTestSwitch()) || CricketConfig.run_mode == MODE_LISTENER) {
    	    				TOSH_CLR_US_IN_EN_PIN();
	    				TOSH_SET_BAT_MON_PIN();
    			} else {
    	    				TOSH_SET_US_IN_EN_PIN();
	    				TOSH_CLR_BAT_MON_PIN();
			}
    			for(i=0;i<8;i++) {
        			beaconmsg.data[i] = CricketConfig.spaceid[i] = tmp->spaceid[i];
			}
        		beacondata->x = CricketConfig.x;
        		beacondata->y = CricketConfig.y;
        		beacondata->z = CricketConfig.z;
    			if (CricketConfig.use_temp_sensor) {
				    call OnboardTempcontrol.start();
	    			    call OnboardTemp.getData();
			}
		}
	}
	// LEDUIT call Leds.redOff();
	return SUCCESS;
  }

  //event ASCII_MsgPtr UARTReceive.receive(ASCII_MsgPtr data) {
  event result_t Serial.Receive(char * buf, uint8_t data_len) {
    const char *pos;
    char OutputMsg[128];
    char tmp[128];
    int c = 1;


    // Prepare the message for parsing if valid
    if (data_len < 4 || data_len > 127) {
	sprintf(OutputMsg,"Err: 1 (Invalid entry)\r\n");
        UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	return SUCCESS;
    }
    strncpy(tmp,buf,4);
    pos = strupr(tmp);
    strncpy(tmp+4,buf + 4,data_len - 4);
    tmp[data_len] = '\0';
    pos = tmp;
    
    sprintf(OutputMsg,tmp);
	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	
    // GET 

	
    if (!strncmp("G", pos, 1)) {
	// Get the hardware serial id of the local node
    	if (!strncmp("ID", pos+2 , 2)) {
       		sprintf(OutputMsg,"ID %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\r\n",
			serialId[0],serialId[1],
	                serialId[2],serialId[3],
	                serialId[4],serialId[5],
	                serialId[6],serialId[7]);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
		else if (!strncmp("TE", pos+2, 2))
				{
				beaconmsg.data[0] = 55;
				beaconmsg.data[1] = 89;
				beaconmsg.data[2] = 99;
				//sprintf(OutputMsg,"test %s", "success");
				//UARTOutput(DBG_ERROR,"%s",OutputMsg);
				sprintf(OutputMsg,"ID %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\r\n",
			serialId[0],serialId[1],
	                serialId[2],serialId[3],
	                serialId[4],serialId[5],
	                serialId[6],serialId[7]);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
				call RadioSend.send(&beaconmsg);
				}
	// Get the protocol version
    	else if (!strncmp("VR", pos+2 , 2)) {
      	      	sprintf(OutputMsg,"VR %s\r\n",
			CricketConfig.version);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get the space id
    	else if (!strncmp("SP", pos+2 , 2)) {
       	     	sprintf(OutputMsg,"SP %s\r\n",
			CricketConfig.version);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get the maximum ultrasound travel time
    	else if (!strncmp("UL", pos+2 , 2)) {
		sprintf(OutputMsg,"UL %d us\r\n",
			CricketConfig.max_us_travel_time);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get the maximum ultrasound travel time
    	else if (!strncmp("UG", pos+2 , 2)) {
		sprintf(OutputMsg,"UG %d\r\n",
			CricketConfig.ultrasound_gain);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get the software offset
    	else if (!strncmp("OF", pos+2 , 2)) {
      	      	sprintf(OutputMsg,"OF %i\r\n",
			CricketConfig.timer_offset);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get local temperature
    	else if (!strncmp("TP", pos+2 , 2)) {
		if (CricketConfig.temp_units == TEMP_C){
			sprintf(OutputMsg,"TP %d C\r\n",
				(uint16_t) CricketConfig.local_temp);
		} else {
			sprintf(OutputMsg,"TP %d F\r\n",
				(uint16_t) (1.8 * CricketConfig.local_temp + 32));
		}
	
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get tinyos local time 
    	else if (!strncmp("TS", pos+2 , 2)) {
      	      	sprintf(OutputMsg,"TS %lu\r\n",
			call Time.getLow32());
	      	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
        // Get test switch status
    	else if (!strncmp("TB", pos+2 , 2)) {
		if (readTestSwitch()) {
      	      		sprintf(OutputMsg,"TB On\r\n");
		} else {
      	      		sprintf(OutputMsg,"TB Off\r\n");
		}
	      	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
	// Get the minimum and maximum beacon interval time
    	else if (!strncmp("SL", pos+2 , 2)) {
		if (CricketConfig.time_units == TIME_US) strncpy(tmp,"us\0",3);
		if (CricketConfig.time_units == TIME_MS) strncpy(tmp,"ms\0",3); 
		if (CricketConfig.time_units == TIME_SS) strncpy(tmp,"ss\0",3);
		    
       		sprintf(OutputMsg,"SL %i %i %s\r\n",
			CricketConfig.min_beacon_interval_time, 
			CricketConfig.max_beacon_interval_time,
			tmp
			);
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Get display units (distance and temperature)
	else if (!strncmp("UN", pos+2, 2)) {
		if (CricketConfig.temp_units == TEMP_C ) strncpy(tmp,"Celsius\0",8);
		if (CricketConfig.temp_units == TEMP_F ) strncpy(tmp,"Fahrenheit\0",11);
		if (CricketConfig.distance_units == DIST_IMPERIAL ) {
       		    sprintf(OutputMsg,"UN Imperial and %s\r\n", tmp);
		} else {
       		    sprintf(OutputMsg,"UN Metric and %s\r\n", tmp);
		}
		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
	// Get the node configuration
        else if (!strncmp("CF", pos+2 , 2)) {
		int cf_pos = 0;
		cf_pos = sprintf(OutputMsg,"\r\nCricket configuration:\r\nSoftware version: %s (%s %s)\r\n", CRICKET_SOFTWARE_VERSION, __DATE__, __TIME__);
		if (CricketConfig.run_mode == MODE_LISTENER) 
     	     		cf_pos += sprintf(OutputMsg + cf_pos ,"Mode: Listener\r\n");
		if (CricketConfig.run_mode == MODE_BEACON) 
       	     		cf_pos += sprintf(OutputMsg + cf_pos ,"Mode: Beacon\r\n");
      	    	cf_pos += sprintf(OutputMsg + cf_pos,"Unique id: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\r\n",
		 			serialId[0],serialId[1],
	                		serialId[2],serialId[3],
	                    		serialId[4],serialId[5],
	                    		serialId[6],serialId[7]);
     	     	cf_pos += sprintf(OutputMsg + cf_pos,"Space id: %s\r\n",
			    	CricketConfig.spaceid);
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		if (CricketConfig.run_mode == MODE_BEACON) { 
     	     		cf_pos = sprintf(OutputMsg ,"Coordinates: (%d,%d,%d)\r\n",
				CricketConfig.x, 
				CricketConfig.y,
				CricketConfig.z
				);
                	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		}
		if (TRUE) {
			uint32_t current_time = 0;
			uint16_t hh = 0;
			uint16_t mm = 0;
			uint16_t ss = 0;
			current_time = call Time.getLow32(); 
			//current_time = 3600000 * 210 + 60000 * 58 + 43000;
			// This generate warning but I could find a ways to remove the warning
			// and have the right result. If you try to fix this use the current_time 
			// commented out just above.
			hh = current_time / 3600000;
			mm = (current_time % 3600000) / 60000;
			ss = ((current_time % 3600000) % 60000) / 1000;
      	      		cf_pos = sprintf(OutputMsg ,"Uptime(hh:mm:ss): %02d:%02d:%02d\r\n",
			    	hh,mm,ss);
                	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		}
      	      	cf_pos = sprintf(OutputMsg,"Ultrasound attenuation time(us): %u\r\n",
			CricketConfig.max_us_travel_time);
      	      	cf_pos += sprintf(OutputMsg + cf_pos,"Ultrasound circuit gain: %u\r\n",
			CricketConfig.ultrasound_gain);
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
      	      	cf_pos = sprintf(OutputMsg,"Timer Offset(us): %u\r\n",
			CricketConfig.timer_offset);
      	      	cf_pos += sprintf(OutputMsg + cf_pos,"Minimum beacon interval(ms): %u\r\n",
			CricketConfig.min_beacon_interval_time);
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
      	      	cf_pos = sprintf(OutputMsg,"Maximum beacon interval(ms): %u\r\n",
			CricketConfig.max_beacon_interval_time);
      	      	cf_pos += sprintf(OutputMsg + cf_pos,"Average beacon interval(ms): %u\r\n",
			CricketConfig.avg_beacon_interval_time);
      	      	cf_pos += sprintf(OutputMsg + cf_pos,"Compensation value(us): %i\r\n",
			CricketConfig.compensation_val);
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		if (CricketConfig.distance_units == DIST_METRIC) {
			cf_pos = sprintf(OutputMsg,"Distance Units: Metric\r\n");
		} else {		
			cf_pos = sprintf(OutputMsg,"Distance Units: Imperial\r\n");
		}
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		if (CricketConfig.use_temp_sensor) {
			if (CricketConfig.temp_units == TEMP_C) {
      	      			cf_pos = sprintf(OutputMsg,"Local temperature value(Celsius): %d\r\n", 
			    		(uint16_t) CricketConfig.local_temp);
			} else {
      	      			cf_pos = sprintf(OutputMsg,"Local temperature value(Fahrenheit): %d\r\n", 
			    		(uint16_t) (1.8 * CricketConfig.local_temp + 32));
			}	
			if (CricketConfig.distance_units == DIST_METRIC) {
	      	      		cf_pos += sprintf(OutputMsg + cf_pos,"Speed of sound value(m/s): %d\r\n",
	  			(uint16_t) (331.5 + 0.60 * CricketConfig.local_temp));
			} else {
	      	      		cf_pos += sprintf(OutputMsg + cf_pos,"Speed of sound value(feet/s): %d\r\n",
	  			(uint16_t) ((331.5 + 0.6 * CricketConfig.local_temp) * 3.281) );
			}
		} else {
      	      			cf_pos = sprintf(OutputMsg,"Local temperature value(Celsius): disabled\r\n");
      	      			cf_pos += sprintf(OutputMsg + cf_pos,"Speed of sound value(m/s): not used\r\n");
		}	
			
		if (readTestSwitch()) {
			cf_pos += sprintf(OutputMsg + cf_pos,"Test switch status: On\r\n" );
		} else {
			cf_pos += sprintf(OutputMsg + cf_pos,"Test switch status: Off\r\n" );
		}
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
      	      	cf_pos = sprintf(OutputMsg,"Event output format: %i\r\n",
			    	CricketConfig.outputformat);
		cf_pos += sprintf(OutputMsg + cf_pos,"Output variable(s):");
		if ( CricketConfig.output_vr ) cf_pos += sprintf(OutputMsg + cf_pos," VR");
		if ( CricketConfig.output_id ) cf_pos += sprintf(OutputMsg + cf_pos," ID");
		if ( CricketConfig.output_db ) cf_pos += sprintf(OutputMsg + cf_pos," DB");
		if ( CricketConfig.output_dr ) cf_pos += sprintf(OutputMsg + cf_pos," DR");
		if ( CricketConfig.output_sp ) cf_pos += sprintf(OutputMsg + cf_pos," SP");
		if ( CricketConfig.output_tm ) cf_pos += sprintf(OutputMsg + cf_pos," TM");
		if ( CricketConfig.output_tp ) cf_pos += sprintf(OutputMsg + cf_pos," TP");
		if ( CricketConfig.output_ts ) cf_pos += sprintf(OutputMsg + cf_pos," TS");
		if ( CricketConfig.output_pc ) cf_pos += sprintf(OutputMsg + cf_pos," PC");
		if ( CricketConfig.output_tb ) cf_pos += sprintf(OutputMsg + cf_pos," TB");
		cf_pos += sprintf(OutputMsg + cf_pos,"\r\n");
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
      	      	cf_pos = sprintf(OutputMsg,"\r\nMassachusetts Institute of Technology\r\nhttp://cricket.csail.mit.edu\r\n\r\n");
                UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
	// Get the list of all the beacon heard and there last five distances
        else if (!strncmp("LS", pos+2 , 2) && CricketConfig.run_mode == MODE_LISTENER) {
		uint8_t tmp_id[8];
		uint8_t size = 0;
		uint8_t i;
		uint16_t distance[5];

		call BeaconsManage.ResetBeaconId();
		listing_count = 0;
		
		while (call BeaconsManage.GetNextBeaconId(tmp_id) == SUCCESS) {
			for (i=0;i<5;i++)
				distance[i] = (uint32_t) call BeaconsManage.GetMeasurements(tmp_id, i);
			if (CricketConfig.distance_units == DIST_IMPERIAL) {
				for (i=0;i<5;i++)
					distance[i] = (distance[i] * 0.3937);
			}
				
                        size = sprintf(OutputMsg, "LS %i: %x:%x:%x:%x:%x:%x:%x:%x %i %i %i %i %i\r\n",
				listing_count,
                                tmp_id[0],tmp_id[1],
                                tmp_id[2],tmp_id[3],
                                tmp_id[4],tmp_id[5],
                                tmp_id[6],tmp_id[7],
                                distance[0],
                                distance[1],
                                distance[2],
                                distance[3],
                                distance[4]);
	        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
			listing_count++;
		} 
	} else {
		sprintf(OutputMsg,"Err: 1 (Invalid entry)\r\n");
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
		
    }
    else if (!strncmp("P", pos, 1)) {
	// Set the run mode
	if (!strncmp("MD", pos+2 , 2)) {
		c = atoi(pos+4);
		// Beacon mode
		if (c == MODE_BEACON) {
		  if (CricketConfig.run_mode != MODE_BEACON) {
	    		call BeaconsControl.stop();
			CricketConfig.run_mode = MODE_BEACON;
			call BeaconTimer.start(TIMER_ONE_SHOT, 2000);
		  }	
    		  // LEDUIT call Leds.yellowOff();
	    	  // LEDUIT call Leds.greenOff();
    	   	  // LEDUIT call Leds.redOff();
      	      	  sprintf(OutputMsg,"MD BEACON\r\n");
		  UARTOutput(DBG_ERROR,"%s",&OutputMsg);
		} else if (c == MODE_LISTENER) {
		  if (CricketConfig.run_mode != MODE_LISTENER) {
			call BeaconTimer.stop();
			CricketConfig.run_mode = MODE_LISTENER;
	    		call BeaconsControl.start();
    	    		TOSH_CLR_US_IN_EN_PIN();
	    		TOSH_SET_BAT_MON_PIN();
		  }
    		  // LEDUIT call Leds.yellowOff();
	    	  // LEDUIT call Leds.greenOff();
    	   	  // LEDUIT call Leds.redOff();
      	      	  sprintf(OutputMsg,"MD LISTENER\r\n");
		  UARTOutput(DBG_ERROR,"%s",&OutputMsg);
		} else {
			sprintf(OutputMsg,"Err: 4\r\n");
        		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		}
	
	}
	// Set output format
    	else if (!strncmp("OU", pos+2 , 2)) {
		uint8_t format = 0;
		format = atoi(pos+4);
		if (format < 5) {
			CricketConfig.outputformat = atoi(pos+4);
      		      	sprintf(OutputMsg,"OU %d\r\n",
				CricketConfig.outputformat);
        		UARTOutput(DBG_ERROR,"%s", &OutputMsg);;
		} else {
      		      	sprintf(OutputMsg,"Err: 3\r\n");
	        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);;
		}
    	}
	// Set offset
    	else if (!strncmp("OF", pos+2 , 2)) {
		CricketConfig.timer_offset = atoi(pos+4);
		CricketConfig.max_allowed_timer_val = CricketConfig.max_us_travel_time + CricketConfig.timer_offset;
      	      	sprintf(OutputMsg,"OF %d\r\n",
			CricketConfig.timer_offset);
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	else if (!strncmp("SV", pos+2 , 2)) {
		call WriteData.write(0, &CricketConfig.version[0], sizeof(struct CricketConfig_s));
		// LEDUIT call Leds.redOn();
		sprintf(OutputMsg,"SV\r\n");
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
	else if (!strncmp("LD", pos+2 , 2)) {
		call ReadData.read(0, &tmpConfig[0], sizeof(struct CricketConfig_s));
      	      	sprintf(OutputMsg,"LD\r\n");
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	} 
	else if (!strncmp("UG", pos+2 , 2)) {
		uint8_t val;
	        val = atoi(pos+4);	
		if (val < 255) {
			call UltrasoundControl.SetGain(val);
			CricketConfig.ultrasound_gain = val;
      	      		sprintf(OutputMsg,"UG %i\r\n",val);
		} else {
      	      		sprintf(OutputMsg,"Err: 2 (Value out of range)\r\n");
		}
	        UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	} 
	// Set the ultrasound maximum time to live 
    	else if (!strncmp("UL", pos+2 , 2)) {
		CricketConfig.max_us_travel_time = atoi(pos+4);
		CricketConfig.max_allowed_timer_val = CricketConfig.max_us_travel_time + CricketConfig.timer_offset;
      	      	sprintf(OutputMsg,"UL %u\r\n",
			CricketConfig.max_us_travel_time);
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Custom output 
    	else if (!strncmp("CO", pos+2 , 2)) {
		if (data_len > 7) {
			CricketConfig.output_vr = FALSE;
			CricketConfig.output_id = FALSE;
			CricketConfig.output_db = FALSE;
			CricketConfig.output_dr = FALSE;
			CricketConfig.output_sp = FALSE;
			CricketConfig.output_tm = FALSE;
			CricketConfig.output_tp = FALSE;
			CricketConfig.output_ts = FALSE;
			CricketConfig.output_pc = FALSE;
		}
		pos = strupr(tmp);
		c = 5;
		while (c < data_len) {
			if (!strncmp("VR", pos+c,2)) {
				CricketConfig.output_vr = TRUE;
			} else if (!strncmp("ID", pos+c,2)) {
				CricketConfig.output_id = TRUE;
			} else if (!strncmp("DB", pos+c,2)) {
				CricketConfig.output_db = TRUE;
			} else if (!strncmp("DR", pos+c,2)) {
				CricketConfig.output_dr = TRUE;
			} else if (!strncmp("SP", pos+c,2)) {
				CricketConfig.output_sp = TRUE;
			} else if (!strncmp("TM", pos+c,2)) {
				CricketConfig.output_tm = TRUE;
			} else if (!strncmp("TP", pos+c,2)) {
				CricketConfig.output_tp = TRUE;
			} else if (!strncmp("TS", pos+c,2)) {
				CricketConfig.output_ts = TRUE;
			} else if (!strncmp("PC", pos+c,2)) {
				CricketConfig.output_pc = TRUE;
			} else if (!strncmp("TB", pos+c,2)) {
				CricketConfig.output_tb = TRUE;
			} else if (!strncmp("*", pos+c,1)) {
				CricketConfig.output_vr = TRUE;
				CricketConfig.output_id = TRUE;
				CricketConfig.output_db = TRUE;
				CricketConfig.output_dr = TRUE;
				CricketConfig.output_sp = TRUE;
				CricketConfig.output_tm = TRUE;
				CricketConfig.output_tp = TRUE;
				CricketConfig.output_ts = TRUE;
				CricketConfig.output_pc = TRUE;
				CricketConfig.output_tb = TRUE;
			} else if (!strncmp("-", pos+c,1)) {
				CricketConfig.output_vr = TRUE;
				CricketConfig.output_id = TRUE;
				CricketConfig.output_db = TRUE;
				CricketConfig.output_dr = TRUE;
				CricketConfig.output_sp = TRUE;
				CricketConfig.output_tm = TRUE;
				CricketConfig.output_tp = FALSE;
				CricketConfig.output_ts = TRUE;
				CricketConfig.output_pc = FALSE;
				CricketConfig.output_tb = FALSE;
			}
			c = c+3;
		}
      	      	sprintf(OutputMsg,"CO %s\r\n",
			pos+5);
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    	}
	// Set space id
	else if (!strncmp("SP", pos+2 , 2)) {
		if (data_len - 5 > 8) data_len = 13; 
		for (c=0;c<data_len - 5;c++)
		{
			CricketConfig.spaceid[c] = *(pos+c+5);
        		beaconmsg.data[c] = CricketConfig.spaceid[c];
		}
		for (c=data_len - 5 ;c<8;c++) {
        		beaconmsg.data[c] = 0x00;
			CricketConfig.spaceid[c] = 0x0;
		}
      	      	sprintf(OutputMsg,"SP %s\r\n",
			CricketConfig.spaceid);
		
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
	// Set the coordinates
	else if (!strncmp("PC", pos+2 , 2)) {
		uint16_t x = 0,y = 0,z = 0;
		c = sscanf(pos+4, "%d %d %d", &x,&y,&z);
		if (c == 3) {
			CricketConfig.x = x;
			CricketConfig.y = y;
			CricketConfig.z = z;
        		beacondata->x = CricketConfig.x;
        		beacondata->y = CricketConfig.y;
        		beacondata->z = CricketConfig.z;
      	      	  	sprintf(OutputMsg,"PC (%d,%d,%d)\r\n",
				beacondata->x = CricketConfig.x,
				beacondata->x = CricketConfig.y,
				beacondata->x = CricketConfig.z);
		  	UARTOutput(DBG_ERROR,"%s",&OutputMsg);
		} else {
			sprintf(OutputMsg,"Err: 2 (Value out of range)\r\n");
        		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
			
		} 
	}	
	else if (!strncmp("SL", pos+2 , 2)) {
                uint16_t max = 0, min = 0;
                c = sscanf(pos+4, "%d %d", &min, &max);
                if(c == 2 && max > min) {
                        CricketConfig.min_beacon_interval_time = min;
                        CricketConfig.max_beacon_interval_time = max;
                        CricketConfig.avg_beacon_interval_time = (min+max) / 2;
                        CricketConfig.delta_beacon_interval_time = max - min;
                        sprintf(OutputMsg,"SL %d %d\r\n",
                                beacondata->x = CricketConfig.min_beacon_interval_time,
                                beacondata->x = CricketConfig.max_beacon_interval_time);
        		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
                } else {
                        sprintf(OutputMsg,"Err: 2 (Value out of range)\r\n");
        		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
                }
        }
	// Put units 
	else if (!strncmp("UN", pos+2, 2)) {
                uint16_t dst = 0, temp = 0;
                c = sscanf(pos+4, "%d %d", &dst, &temp);
		if(c == 2) {
			if (dst == 1) {
				CricketConfig.distance_units = DIST_METRIC;
			} else if (dst == 2) {
				CricketConfig.distance_units = DIST_IMPERIAL;
			}
			if (temp == 1) {
				CricketConfig.temp_units = TEMP_C;
			} else if (temp == 2) {
				CricketConfig.temp_units = TEMP_F;
			}
			if (dst != 1 && dst != 2 && temp != 1 && temp != 2) {
                        	sprintf(OutputMsg,"Err: 2 (Value out of range)\r\n");
        			UARTOutput(DBG_ERROR,"%s", &OutputMsg);
			} else {
				if (CricketConfig.temp_units == TEMP_C ) strncpy(tmp,"Celsius\0",8);
				if (CricketConfig.temp_units == TEMP_F ) strncpy(tmp,"Fahrenheit\0",11);
				if (CricketConfig.distance_units == DIST_IMPERIAL ) {
		       		    sprintf(OutputMsg,"UN imperial and %s\r\n", tmp);
				} else {
		       		    sprintf(OutputMsg,"UN metric and %s\r\n", tmp);
				}
        			UARTOutput(DBG_ERROR,"%s", &OutputMsg);
			}
		} else {
                        sprintf(OutputMsg,"Err: 2\r\n");
        		UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		}
	}
	// Enable onboard temperature sensor
	else if (!strncmp("TP", pos+2 , 2)) {
		c = atoi(pos+4);
		if (c == 0 || CricketConfig.use_temp_sensor) {
		  CricketConfig.use_temp_sensor = FALSE; 
	          call OnboardTempcontrol.stop();
		} else if (c > 0 || !CricketConfig.use_temp_sensor) {	
		  CricketConfig.use_temp_sensor = TRUE;
	          call OnboardTempcontrol.start();
	          call OnboardTemp.getData();
		} 
      	      	sprintf(OutputMsg,"TP %d\r\n",
			CricketConfig.use_temp_sensor);
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
		
	} else {
		sprintf(OutputMsg,"Err: 1\r\n");
        	UARTOutput(DBG_ERROR,"%s", &OutputMsg);
	}
	
	
    } else {
	sprintf(OutputMsg,"Err: 1\r\n");
        UARTOutput(DBG_ERROR,"%s", &OutputMsg);
    }
    return SUCCESS;
  }

}


